#ifndef KALMAN_SCAN_H
#define KALMAN_SCAN_H
#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include "ros/ros.h"
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <limits>
#include <sensor_msgs/LaserScan.h>
#include <function_test/ScanPlot.h> 
class KalmanFilter {
public:
    KalmanFilter(double process_noise, double measurement_noise, double estimation_error, double initial_value) {
        Q = process_noise;
        R = measurement_noise;
        P = estimation_error;
        X = initial_value;
    }
	void setInitial_value(float initial_value){
		X = initial_value;
	}
    double update(double measurement) {
        // 预测更新
        P = P + Q;

        // 计算卡尔曼增益
        K = P / (P + R);

        // 更新估计值
        X = X + K * (measurement - X);

        // 更新估计误差
        P = (1 - K) * P;

        return X;
    }

private:
    double Q; // 过程噪声协方差
    double R; // 测量噪声协方差
    double P; // 估计误差协方差
    double K; // 卡尔曼增益
    double X; // 估计值
};

#endif // KALMAN_SCAN_H